/*
 * trajectory.cpp
 *
 *  Created on: 2020年12月2日
 *      Author: Huanyu Zhang
 */
#include "trajectory.hpp"
#include "control/control_attitude.hpp"

TRAJECTORY trajectory;

void TRAJECTORY::Trajectory_Step()
{
	switch (trajectory_mod)
	{
		case Hover : Trajectory_Hover_Mode_Step();//
		break;

		case Point_to_point : Trajectory_Point_to_point_Mode_Step();//
		break;

		case Circle : Trajectory_Circle_Mode_Step();//
		break;
//
//		case Mixture : Trajectory_Mixture_Mode_Step();//
//		break;
//
//		case Test : Trajectory_Test_Mode_Step();//
//		break;
//
//		case RTL : Trajectory_RTL_Mode_Step(); //return-to-launch 自动返航模式，返航后在起飞点一直悬停，直到燃料耗尽
//		break;

		case Dubins: Trajectory_Dubins_Mode_Step(); //
		break;

		case MinimumSnap: Trajectory_MinimumSnap_Mode_Step();
		break;

		default : Trajectory_Brake_Down_Step();	//
		break;
	}
}


void TRAJECTORY::Trajectory_Hover_Mode_Step()
{
	if (heli_step == hoverStep) //heli_step == 1，悬停模式
		Trajectory_Hover_Step();
	if (hover_time <= 0)
		hover_time = FLY_TIME; //悬停0.01 * 500 = 5s
}

void TRAJECTORY::Trajectory_Hover_Step()
{
	for(int i = 0; i < 3; ++i)
	{
		com_pqr[i] = 0.0f;
		com_theta[i] = 0.0f;
		cur_target_vel_ned[i] = 0.0f;
		cur_target_vel_body[i] = 0.0f;
		top_speed_body[i] = 0.0f;
		how_long_body[i] = 0.0f;
	}
	hover_time--;
	if(hover_time < -500)
		hover_time = -200;
}

void TRAJECTORY::Trajectory_Point_to_point_Mode_Step()
{
//	if(3 == auto_pilot11)
//	{	//未用到，应该是一键起飞
//		next_target_ned[0] = hover_ned[0]; //hover_ned[0]取的是当前GPS算出来的NED坐标
//		next_target_ned[1] = hover_ned[1];
//		next_target_ned[2] = TAKE_OFF_HEIGHT;   //,TAKE_OFF_HEIGHT
//		hover_time = DEFAULT_HOVER_TIME;
//		Trajectory_Schedule(ele,MAX_COM_SPEED, Point_to_point);//计算当前位置与目标位置之间的距离
//		heli_step = 5;
//		auto_pilot11 = 0;
//	}
//	else if((2 == auto_pilot11) && (hover_time <=-1))
//	{
//		next_target_ned[0] = cur_target_pos_ned[0];
//		next_target_ned[1] = cur_target_pos_ned[1];
//		next_target_ned[2] = -1.0f;   //1m
//		hover_time = 400;//4s
//		Trajectory_Schedule(ele,MAX_COM_SPEED, Point_to_point);
//		if(fabs(how_long_body[2]) < 0.4f)
//		{
//			heli_step = 1;
//			Trajectoyr_Current_Target_Update(ele,next_target_ned);
//		}
//		else
//		{
//			heli_step = 5;
//		}
//		auto_pilot11 = 4;
//	}
//	else if((4 == auto_pilot11) && (hover_time <=-1))
//	{	//在下一个目标点降落
//		next_target_ned[0] = cur_target_pos_ned[0];
//		next_target_ned[1] = cur_target_pos_ned[0];
//		next_target_ned[2] = 0.0f;
//		hover_time = 200;
//		how_long_body[2] = -1;
//		heli_step = 7;
//		auto_pilot11 = 6;
//	}
    if (Trajectory_Target_Update()) //当目标队列非空时，将next_target_ned时更新为绝对坐标，返回true；当目标队列空时，返回false
	{
		Trajectory_Schedule(MAX_COM_SPEED); //计算how_long_body、top_speed_body
		if (how_long_body[0] > BOUNDARY_DISTANCE)
		{	//如果发现next_target_ned与当前cur_target_pos_ned之间的水平距离大于BOUNDARY_DISTANCE == 2m，那么调转机头，机头方向朝向next_target_ned方向
			Trajectory_Turn_Heading(&end_heading); //计算机头期望航向角
			heli_step = turnHeadStep;
		}
		else if (how_long_body[0] <= BOUNDARY_DISTANCE) //待优化，现在的做法是什么都不做
		{
			for(int i = 0; i < 3; ++i){
				cur_target_pos_ned[i] = next_target_ned[i];
			}
			xQueuePeek(queueESKF, &eskf, 0);
			end_heading = eskf.Attitude[2];
			heli_step = hoverStep; //悬停
		}
	}
	if (heli_step == hoverStep)   //heli_step == 1时表示处于悬停状态
	{
		Trajectory_Hover_Step();
	}
	else if (heli_step == turnHeadStep) //heli_step == 2 时表示处于调头状态
	{
		Trajectory_Turning_Head_Step();
		if(turn_accel_flag==true) //调转机头成功标志位
		{
			turn_accel++;
		}
		if(turn_accel==300) //在调转机头成功后，再等待10ms * 300 = 3s
		{
			turn_accel=0;
			turn_accel_flag=false;
			heli_step = speedUpStep;
		}
	}
	else if (heli_step == speedUpStep) //heli_step == 3代表加速阶段
	{
		if (fabs(how_long_body[0]) <= 0.5f * top_speed_body[0] * top_speed_body[0] / ACCEL) //如果剩余水平距离<=0.5f * top_speed_body[0] * top_speed_body[0]，那么应该减速
			heli_step = speedDownStep;
		else
			Trajectory_Speed_Up_Step();
	}
	else if (heli_step == speedDownStep) //heli_step == 4代表减速阶段
	{
		if ((how_long_body[0]) <= 0.05f || cur_target_vel_body[0] == 0.0f)
		{	//如果当前距离小于0.05m或者说当前水平速度到达了0，则说明已经到达了目标点(即使未达到目标点，也认为它到达了)，进入悬停模式
			how_long_body[0] = 0.0f ;
			heli_step = hoverStep;
			for(int i=0 ; i!=3 ; ++i) cur_target_pos_ned[i] = next_target_ned[i];
		}
		else
		{	//否则做匀减速运动
			Trajectory_Speed_Down_Step(); //
		}
	}
//////////////////////////////////////////////////////////////////////////////////
//	if (heli_step == 5) //
//	{
//		/************************2015-12-18**********************************/
//		if (fabs(how_long_body[2]) <= 0.25f * distance_vertical) //1/4
//			heli_step = 6;
//		else
//			Trajectory_Speed_Up_Step_Vertical(ele);        //
//	}
//
//	if (heli_step == 6) //
//	{
//		/************************2015-12-18**********************************/
//		if (fabs(how_long_body[2]) <= 0.05f || cur_target_vel_body[2] == 0.0f) //1/4
//		{	//当高度值<= 0.05或者说机身垂直速度为0，认为飞机已经停止
//			heli_step = 1; //飞机进入悬停状态
//			Trajectoyr_Current_Target_Update(ele,next_target_ned); //更新cur_target_pos_ned
//		}
//		else
//		{
//		  Trajectory_Speed_Down_Step_Vertical(ele);        //
//		}
//	}
////////////////////////////////////////////////////////////////////////////////////
//	if (heli_step == 7)//
//	{
//		if(fabs(how_long_body[2]) <= 0.01f)
//		{	//悬停
//			heli_step = 1;
//			auto_pilot11 = 7;//
//			Trajectoyr_Current_Target_Update(ele,next_target_ned);
//		}
//		else
//			Trajectory_Land_Step(ele);
//	}
////////////////////////////////////////////////////////////////////////////////////
	if (heli_step == brakeDownStep)
	{
		Trajectory_Brake_Down_Step();
	}
}

void TRAJECTORY::Trajectory_Circle_Mode_Step()
{
	if (Trajectory_Target_Update())
	{
		Trajectory_Schedule(circle_velocity);
		if (how_long_body[0] > 0.5f * circle_velocity * circle_velocity / ACCEL)
		{	//如果两点距离大于一定值(4.5m)，那么掉头，使得机头指向next_target_ned
			Trajectory_Turn_Heading(&end_heading);
			heli_step = turnHeadStep;
		}
		else
		{
			for(int i = 0; i < 3; ++i){
				cur_target_pos_ned[i] = next_target_ned[i];
			}
			xQueuePeek(queueESKF, &eskf, 0);
			end_heading = eskf.Attitude[2];
			heli_step = hoverStep; //悬停
		}
	}
	if (heli_step == hoverStep)
	{
		Trajectory_Hover_Step();
	}
	else if (heli_step == turnHeadStep)
	{
		Trajectory_Turning_Head_Step();
		if(turn_accel_flag==true)
		{
		  turn_accel++;
		}
		if(turn_accel == 300)//3
		{
			turn_accel=0;
			turn_accel_flag=false;
			heli_step = speedUpStep;
		}
	}
	else if (heli_step == speedUpStep)
	{
		Trajectory_Speed_Up_Step();
		if (fabs(how_long_body[0]) <= 0.2f)
		{
			heli_step = circleStep;
			circle_finish_flag = false;
			for(int i = 0; i < 3; ++i){
				cur_target_pos_ned[i] = next_target_ned[i];
			}
		}
	}
	else if (heli_step == circleStep && circle_finish_flag == false)
	{
		circle_turn = leftCircle; //
		//圆弧轨迹核心函数，实现的是到达next_target_ned后，以半径5m，线速度3m/s做圆周运动，画circleNum圈
		Trajectory_Circle_Step(circle_velocity, circle_radius_test, 0.0f, 360.0f * D2R * circleNum);
		return;
	}
	else if (heli_step == brakeDownStep)
	{
		Trajectory_Brake_Down_Step();
	}
}


void TRAJECTORY::Trajectory_Circle_Step(float _circle_velocity, float _radius,float _helix_velocity, float _angle)
{
	static float init_theta,init_position[3];
	xQueuePeek(queueESKF, &eskf, 0);
	if (first_entry_circle == 1)
	{
		cur_target_vel_body[2] = -_helix_velocity;
		cur_target_vel_body[0] = _circle_velocity;
		com_pqr_ned[2] = cur_target_vel_body[0] / _radius; //求出角速度，是绕与D平行的轴转动，因此角速度应该是NED的Z轴角速度

		circle_angle = 0.0; //,
		init_theta = yaw_command; //
		init_position[0] = cur_target_pos_ned[0]; // current_xyz
		init_position[1] = cur_target_pos_ned[1];
		init_position[2] = cur_target_pos_ned[2];
	}
	circle_angle += com_pqr_ned[2]*TRAJECTORY_CONTROL_DT; //圆心角
	if(circle_angle >= _angle)
	{	//如果超过了预定的角度值，那么清零所有值
		for(int i = 0; i < 3; ++i)
		{
			com_pqr[i] = 0.0;
			com_theta[i] = 0.0;
			top_speed_body[i] = 0.0;
			how_long_body[i] = 0.0;
		}
		cur_target_vel_body[2] = 0.0;
		circle_finish_flag = true;
		first_entry_circle = 1;
		first_entry_brake = 1;
		heli_step = brakeDownStep;//进入刹车模式
		return;
	}
	else
	{
		/*
		 * bonus:利用com_pqr[2]作为期望值，GyrFil_2nd[2]作为反馈值，得到的结果经过P控制器，传入auto_servo_width[3]
		 * output = (com_pqr[2] - GyrFil_2nd[2]) * K;
		 * auto_servo_width[3] += / -= output;
		 * GyrFil_2nd[2]跟不上com_pqr[2]时，需要更向一侧偏移；否则需要向中间靠拢
		 * */
		com_pqr[2] = com_pqr_ned[2] * cos(eskf.Attitude[0]) * cos(eskf.Attitude[1]);
	}

	if (circle_turn == rightCircle)//
	{
		yaw_command += com_pqr[2]*TRAJECTORY_CONTROL_DT;
//		yaw_command += com_pqr_ned[2]*TRAJECTORY_CONTROL_DT;
		if (yaw_command > PI)
		{
		  yaw_command -= 2*PI;
		}
		//陈宏润论文P83
		cur_target_pos_ned[0] = init_position[0]+_radius*(-sin(init_theta)+sin(yaw_command));
		cur_target_pos_ned[1] = init_position[1]+_radius*(cos(init_theta)-cos(yaw_command));

		cur_target_vel_ned[0] = cur_target_vel_body[0] * cos(yaw_command);
		cur_target_vel_ned[1] = cur_target_vel_body[0] * sin(yaw_command);
	}
	else if (circle_turn == leftCircle)//
	{
		yaw_command -= com_pqr[2]*TRAJECTORY_CONTROL_DT;
//		yaw_command -= com_pqr_ned[2]*TRAJECTORY_CONTROL_DT;
		if (yaw_command < -PI)
		{
			yaw_command += 2*PI;
		}

		cur_target_pos_ned[0] = init_position[0]+_radius*(sin(init_theta)-sin(yaw_command));
		cur_target_pos_ned[1] = init_position[1]+_radius*(-cos(init_theta)+cos(yaw_command));

		cur_target_vel_ned[0] = cur_target_vel_body[0] * cos(yaw_command);
		cur_target_vel_ned[1] = cur_target_vel_body[0] * sin(yaw_command);
	}
	cur_target_pos_ned[2] += cur_target_vel_body[2] * TRAJECTORY_CONTROL_DT ;
	first_entry_circle++;
	if(first_entry_circle >= 10000)
		first_entry_circle = 200;
}

bool TRAJECTORY::Trajectory_Target_Update()
{
	if (trajectory_mod == Point_to_point)
	{
		if ((hover_time <=-1 && heli_step == hoverStep && (!Target_Queue_Is_Queue_Empty())) || first_receive_target == 1)
		{	//两个触发条件：
			//第一个是当飞机已经悬停了一段时间（5s以上），并且剩余队列目标点不为空，那么更新next_target_ned，进行下一段飞行；
			//第二个是第一次从地面站接收到传来的目标点时，更新next_target_ned
			if(!Pop_Next_Target(next_target_ned,&hover_time))  //如果队列空了，那么返回false；否则将队列的第一个[x,y,z]与hover time存入next_target_ned与hover_time
				return false;					//若没有检测到目标序列有出栈，则return false
			Target_Plus_Hover_Point();   //我们设定的目标点是相对于机体当前所在的位置来说的，因此要加上此时的悬停点的坐标位置
			first_receive_target++;
			return true;
		}
		else if (Target_Queue_Is_Queue_Empty() && hover_time <= -2000)  //当队列为空时，则目标点转化为悬停点，返航，永远不可能实现，因为hover_time > -500
		{
			next_target_ned[0] = hover_ned[0] ;
			next_target_ned[1] = hover_ned[1] ;
			next_target_ned[2] = hover_ned[2] ;
			hover_time = DEFAULT_HOVER_TIME ;
			return true;
		}
	}
	else if (trajectory_mod == Circle)
	{
		//更新next_target_ned的两种情况：
		//第一种情况：第一次从地面站接收到传来的目标点
		//第二种情况：飞机已经完成了上一次的圆弧飞行，进行下一次圆弧飞行
		if ((!Target_Queue_Is_Queue_Empty()) && first_receive_target == 1)
		{
			if(!Pop_Next_Target(next_target_ned,&hover_time))
				return false;
			Target_Plus_Hover_Point();
			first_receive_target++;
			return true;
		}
		else if ((!Target_Queue_Is_Queue_Empty()) && circle_finish_flag == true && hover_time <=-1)
		{
			if(!Pop_Next_Target(next_target_ned,&hover_time))
				return false;
			Target_Plus_Hover_Point();
			circle_finish_flag = false;
			return true;
		}
	}

//	if (trajectory_mod == Mixture)
//	{
//		if (circle_finish_flag == true)
//		{
//			for (int i=0; i!=5; i++)
//			{
//				next_target_ned[i] = next_next_target[i];
//			}
//			if(!Target_Queue_Pop_Target(next_next_target,&hover_time))
//			{
//				heli_step = 8;
//				return false;
//			}
//			Target_Plus_Hover_Point(ele);
//			return true;
//		}
//		if ((!Target_Queue_Is_Queue_Empty()) && first_receive_target == 1)
//		{
//			if(!Target_Queue_Pop_Target(next_target_ned,&hover_time))
//				return false;
//			Target_Plus_Hover_Point(ele);
//			if(!Target_Queue_Pop_Target(next_next_target,&hover_time))
//				return false;
//			Target_Plus_Hover_Point(ele);
//			first_receive_target++;
//			return true;
//		}
//	}
	return false;
}

void TRAJECTORY::Target_Plus_Hover_Point()
{
	for(int i=0;i<3;i++)
	{
		next_target_ned[i] = next_target_ned[i] + hover_ned[i]; //next_target_ned实际上记录的是地面站上的期望点与现地点之间的相对位置，如果要得到
	}																		   //绝对位置，那么一定需要将相对位置 + reset时刻的基准点才是
}

void TRAJECTORY::Trajectory_Schedule(int max_speed)  //next_targe
{
	Trajectory_Distance_Calculate();              //更新下一目标点与当前目标点(并非当前飞机的实际位置)的距离，包括水平[0]与垂直[2]距离
	Trajectory_Speed_Calculate(max_speed);        //更新最大的速度值，水平：top_speed_body[0] 竖直：top_speed_body[2]
}


void TRAJECTORY::Trajectory_Distance_Calculate()
{
	if (trajectory_mod == Point_to_point || trajectory_mod == Circle || trajectory_mod == RTL)
	{
		how_long_body[0] =sqrt((cur_target_pos_ned[0]-next_target_ned[0])*(cur_target_pos_ned[0]-next_target_ned[0]) //计算出当前通过GPS解算出来的位置cur_target_pos_ned与地面站上传而来的下一个期望点next_target_ned之间的距离
					+ (cur_target_pos_ned[1]-next_target_ned[1])*(cur_target_pos_ned[1]-next_target_ned[1]));
		how_long_body[2] = cur_target_pos_ned[2]-next_target_ned[2];

		vertical_speed_direction = how_long_body[2] >= 0?-1:1;//注意NED坐标，D指向地心，因此向上反而是负
		distance_line = how_long_body[0];
		distance_vertical = fabs(how_long_body[2]);
		return;
	}
//	else if (trajectory_mod == Mixture)
//	{
//		float cross_product = (next_target_ned[0]-cur_target_pos_ned[0])*(next_next_target[1]-cur_target_pos_ned[1])
//			- (next_target_ned[1]-cur_target_pos_ned[1])*(next_next_target[0]-cur_target_pos_ned[0]);
//		if (cross_product == 0)
//		{ //当next_target_ned == cur_target_pos_ned时，说明已经到达了目标点
//			for (int i=0; i!=5; i++)
//			{
//				next_target_ned[i] = next_next_target[i]; //这时候叉积为0，要更新该目标点
//			}
//			if(!Target_Queue_Pop_Target(next_next_target,&hover_time))
//			{ //如果next_target_ned是最后一个在地面站上设定的期望点
//				heli_step = 8;
//				return;
//			}
//			Target_Plus_Hover_Point(ele);
//		}
//		else
//		{
//			if (cross_product < 0)
//			{
//				circle_turn = 2;	//,
//			}
//			else
//			{
//				circle_turn = 1;	//,
//			}
//		}
//
//		how_long_body[0] = sqrt((cur_target_pos_ned[0]-next_target_ned[0])*(cur_target_pos_ned[0]-next_target_ned[0])
//					+ (cur_target_pos_ned[1]-next_target_ned[1])*(cur_target_pos_ned[1]-next_target_ned[1]));
//		how_long_body[2] = hover_ned[2]-next_target_ned[2];
//		//计算本次期望点与下一个期望点之间的距离，当前位置与下一个期望点之间的距离
//		float b = sqrt((next_target_ned[0]-next_next_target[0])*(next_target_ned[0]-next_next_target[0])
//						+ (next_target_ned[1]-next_next_target[1])*(next_target_ned[1]-next_next_target[1]));
//		float c = sqrt((cur_target_pos_ned[0]-next_next_target[0])*(cur_target_pos_ned[0]-next_next_target[0])
//						+ (cur_target_pos_ned[1]-next_next_target[1])*(cur_target_pos_ned[1]-next_next_target[1]));
//
//		sector_angle = acos((pow(how_long_body[0],2)+pow(b,2)-pow(c,2)) / 2*fabs(how_long_body[0]*b));
//		circle_radius = 2*circle_velocity;
//		if (sector_angle >= 0.785f && sector_angle <= 2.356f)//60-120 45 - 135
//		{
//			float r;
//			sector_angle = PI - sector_angle;
//			r = circle_radius*tan(sector_angle/2);
//			how_long_body[0] = how_long_body[0] - r;
//			next_target_ned[0] -= r*cos(ahrs[1].Ang[2]);
//			next_target_ned[1] -= r*sin(ahrs[1].Ang[2]);
//			return;
//		}
//		else
//		{
//			trajectory_mod = Point_to_point;
//			Mixture_to_Pointtopoint_flag = true;
//			return;
//		}
//	}
}

void TRAJECTORY::Trajectory_Speed_Calculate(int max_com_speed)
{
	if (trajectory_mod == Point_to_point || trajectory_mod == RTL )
	{
		if (2.0f * (max_com_speed * max_com_speed) / ACCEL > distance_line)
		{
			top_speed_body[0] = sqrt(ACCEL * distance_line / 2.0f);
		}
		else
		{
			top_speed_body[0] = max_com_speed;
		}
		if (2.0f * (MAX_COM_SPEED_VERTICAL * MAX_COM_SPEED_VERTICAL) / ACCEL_VERTICAL > distance_vertical)
		{
			top_speed_body[2] = sqrt(ACCEL_VERTICAL * distance_vertical / 2.0f);
		}
		else
		{
			top_speed_body[2] = MAX_COM_SPEED_VERTICAL;
		}
		return;
	}
	else if (trajectory_mod == Circle)
	{
		if (0.5f * (max_com_speed * max_com_speed) / ACCEL > distance_line)
		{
//			top_speed_body[0] = sqrt(2 * ACCEL * distance_line);
			top_speed_body[0] = 0.0f; //当加速距离小于4.5m时，不允许进行圆弧飞行
		}
		else
		{
			top_speed_body[0] = max_com_speed;
		}
		return;
	}
//	if (trajectory_mod == Mixture)
//	{
//		if (circle_finish_flag == false)
//		{
//			if (0.5f*(max_com_speed*max_com_speed)/ACCEL > how_long_body[0])
//			{
//				top_speed_body[0] = 0;
//			}
//			else
//			{
//				top_speed_body[0] = max_com_speed;
//			}
//			return;
//		}
//		else
//		{
//			top_speed_body[0] = max_com_speed;
//		}

//	}
}

void TRAJECTORY::Trajectory_Turn_Heading(float * end_heading_point)
{
	float temp = atan2f(next_target_ned[0]-cur_target_pos_ned[0],next_target_ned[1]-cur_target_pos_ned[1]);
	// atan2X,end_headingNED
	if (temp > -PI /2.0f)
		*end_heading_point = PI /2.0f - temp ;
	else
		*end_heading_point = - 3.0f * PI / 2.0f - temp ;

	if (*end_heading_point - yaw_command > PI)
	{
		isturn = 1;     //
		return;
	}
	if (*end_heading_point - yaw_command < -PI)
	{
		isturn = 2;     //
		return;
	}
	if (*end_heading_point - yaw_command > 0.00f && *end_heading_point - yaw_command < PI)
	{
		isturn = 3;     //
		return;
	}
	if (*end_heading_point - yaw_command < 0.00f && *end_heading_point - yaw_command > -PI)
	{
		isturn = 4;    //
		return;
	}
}


void TRAJECTORY::Trajectory_Turning_Head_Step()
{
	if ((fabs(end_heading - yaw_command)) < 0.04f)  //0.04rad 2.3deg
	{ //掉头成功
		isturn = 0 ;
		yaw_command = end_heading; //更新yaw_command，为下次掉头做准备
		turn_accel_flag=true;
		return;
	}
	if (isturn == 1 || isturn ==4)
	{ //这两种情况，机头向左侧转动角度更少
		yaw_command -= 0.003f; //10ms转0.003rad，即0.3rad/s，yaw_command 作为期望值参与角度环计算，进入串极-前馈PI控制
		if (yaw_command< - PI ) //保证yaw_command始终在[-pi,pi]区间内
			yaw_command +=2* PI;
		return;
	}
	else if (isturn == 2 || isturn ==3)
	{ //这两种情况，机头向右侧转动角度更少
		yaw_command += 0.003f;
		if (yaw_command > PI ) //保证yaw_command始终在[-pi,pi]区间内
			yaw_command -=2* PI;
		//position_t->heading += 0.025 ;
		return;
	}
}


void TRAJECTORY::Trajectory_Speed_Up_Step()
{	//做匀加速运动，当加到飞机实际能够达到的最大速度时候，匀速运动
	//并更新与目标点的水平距离
	if (cur_target_vel_body[0] < top_speed_body[0])
	{
		cur_target_vel_body[0] +=  TRAJECTORY_CONTROL_DT * ACCEL ;
	}
	else
	{
		cur_target_vel_body[0] =  top_speed_body[0] ;
	}

	how_long_body[0] -= cur_target_vel_body[0] * TRAJECTORY_CONTROL_DT;
	cur_target_vel_ned[0] = cur_target_vel_body[0] * cos(yaw_command); //应该是yaw_command，还是eskf.Attitude[2]?
	cur_target_vel_ned[1] = cur_target_vel_body[0] * sin(yaw_command);

	cur_target_pos_ned[0] += cur_target_vel_ned[0] * TRAJECTORY_CONTROL_DT;
	cur_target_pos_ned[1] += cur_target_vel_ned[1] * TRAJECTORY_CONTROL_DT;

//	#ifdef circle_test
//	if(cur_target_vel_body[0] >= 0.5)
//	{
//		heli_step = circleStep;
//	  return ;
//	}
//	#endif
}

void TRAJECTORY::Trajectory_Speed_Down_Step()
{
	if ( cur_target_vel_body[0] > 0.0f)
	{
		cur_target_vel_body[0] -=  TRAJECTORY_CONTROL_DT *ACCEL ;
	}
	else
	{
		cur_target_vel_body[0] = 0.0f;
	}
	how_long_body[0] -= cur_target_vel_body[0] * TRAJECTORY_CONTROL_DT;
	cur_target_vel_ned[0] = cur_target_vel_body[0] * cos(yaw_command);
	cur_target_vel_ned[1] = cur_target_vel_body[0] * sin(yaw_command);

	cur_target_pos_ned[0]+= cur_target_vel_ned[0] * TRAJECTORY_CONTROL_DT;
	cur_target_pos_ned[1]+= cur_target_vel_ned[1] * TRAJECTORY_CONTROL_DT;

}

void TRAJECTORY::Trajectory_Brake_Down_Step()
{
	hover_time = DEFAULT_HOVER_TIME;
    //检查是否为first_entry_brake，该值在count == 1时，被初始化为1
	if((first_entry_brake))
	{	//若是，则计算刹车距离，设刹车是匀减速度运动，从刹车开始速度V1 = cur_target_vel_body[0]，加速度是ACCEL
		brake_distance = 0.5f * cur_target_vel_body[0] * cur_target_vel_body[0] / ACCEL;
		first_entry_brake = 0;
		for(int i = 0; i < 3; ++i)
		{
			top_speed_body[i] = 0.0f;
			how_long_body[i] = 0.0f;
		}
	}
	else
	{
		if (cur_target_vel_body[0] > 0)
		{//如果当前机体速度不为零，那么设机体为匀减速运动计算这次机体速度，并以此更新刹车距离、x轴与y轴的位置与速度信息
			cur_target_vel_body[0] -=  TRAJECTORY_CONTROL_DT *ACCEL ; //匀减速运动
			brake_distance = brake_distance - cur_target_vel_body[0]*TRAJECTORY_CONTROL_DT; //简化成匀速直线运动，是否也可以写成：(V1^2 - V0^2) / 2a
			cur_target_vel_ned[0] = cur_target_vel_body[0] * cos(yaw_command); //自动模式下的俯仰角为0，否则还需要乘cos(Heli.pitch_command)
			cur_target_vel_ned[1] = cur_target_vel_body[0] * sin(yaw_command);
			cur_target_pos_ned[0]+= cur_target_vel_ned[0] * TRAJECTORY_CONTROL_DT;
			cur_target_pos_ned[1]+= cur_target_vel_ned[1] * TRAJECTORY_CONTROL_DT;
		}
		else
		{	//如果cur_target_vel_body[0] == 0，说明刹车已经完成，那么飞机进入悬停状态（heli_step = 1）
			heli_step = hoverStep;
			minimumSnapStepFlag = msHoverStep;
			first_entry_brake = 1;
		}
	}
}

void TRAJECTORY::Trajectory_Dubins_Mode_Step(){
	if(dubins_mission_moving){//初始化dubins后，dubins_mission_moving == true
		if(dubins_segment_moving < dubins_segment){
			//dubins_segment_moving:当前已完成的dubins曲线条数
			//dubins_segment:总的dubins曲线条数
			switch (dubins_curve_buffer[dubins_segment_moving].kind){
				case LSL:
					Trajectory_Dubins_LSL_Step();
					break;
				case LSR:
					Trajectory_Dubins_LSR_Step();
					break;
				case RSL:
					Trajectory_Dubins_RSL_Step();
					break;
				case RSR:
					Trajectory_Dubins_RSR_Step();
					break;
				case RLR:
					Trajectory_Dubins_RLR_Step();
					break;
				case LRL:
					Trajectory_Dubins_LRL_Step();
					break;
				case FALSE:
					heli_step = hoverStep;
					Trajectory_Hover_Step();
					break;
			}
		}else{//说明dubins曲线已经完成，dubins曲线进行标志位置false，并且刹车置位
			dubins_mission_moving = false;
			first_entry_brake = 1;
			heli_step = brakeDownStep;
			clearFlag = true;
		}
	}else{
		if(heli_step == brakeDownStep){
			Trajectory_Brake_Down_Step();
		}

		if(heli_step == hoverStep){
			Trajectory_Hover_Step();
		}
	}
}


void TRAJECTORY::Dubins_Planning()
{	//100ms执行一次，10Hz
	if(trajectory_mod == Dubins){
		xQueuePeek(queueTargetPoint, &targetPoint, 0);
		if(Target_Queue_Is_Queue_Empty() && dubins_target_init == false) //还未等到地面站上传的点
			return;
		if(dubins_target_init == false)
		{
			if(dubins_idx >= MAX_QUEUE_SIZE) return; //dubins_target队列坐标
			osDelay(1000); //sleep 1000ms，等待所有点全部传入
			xQueuePeek(queueTargetPoint, &targetPoint, 0); //再次更新队列
			if(dubins_idx == 1){
				dubins_target_buffer[0].x = cur_target_pos_ned[0]; //起飞时的点
				dubins_target_buffer[0].y = cur_target_pos_ned[1];
				dubins_target_buffer[0].yaw_dubins = yaw_command; //以yaw作为alpha角
			}
			if(targetPoint.eptr - targetPoint.fptr == 1){ //如果队列中仅有一个点
				dubins_target_buffer[dubins_idx].x = targetPoint.TargetPointList[targetPoint.fptr][0] + hover_ned[0];
				dubins_target_buffer[dubins_idx].y = targetPoint.TargetPointList[targetPoint.fptr][1] + hover_ned[1];
				dubins_target_buffer[dubins_idx].yaw_dubins = (yawAutoSetFlag == true ? yaw_command : targetPoint.yawCommandList[targetPoint.fptr]);	//如果只有按了一个点，那么输入最后一个点的角度是yaw
				targetPoint.fptr++;
				dubins_idx++;
			}
			else{
				while(targetPoint.eptr - targetPoint.fptr > 1){ //保持至少2个元素存在
					dubins_target_buffer[dubins_idx].x = targetPoint.TargetPointList[targetPoint.fptr][0] + hover_ned[0];
					dubins_target_buffer[dubins_idx].y = targetPoint.TargetPointList[targetPoint.fptr][1] + hover_ned[1];
					dubins_target_buffer[dubins_idx].yaw_dubins = (yawAutoSetFlag == true ? atan2(targetPoint.TargetPointList[targetPoint.fptr + 1][1]-targetPoint.TargetPointList[targetPoint.fptr][1],targetPoint.TargetPointList[targetPoint.fptr + 1][0]-targetPoint.TargetPointList[targetPoint.fptr][0]) : targetPoint.yawCommandList[targetPoint.fptr]);
					targetPoint.fptr++;
					dubins_idx++;
				}
				dubins_target_buffer[dubins_idx].x = targetPoint.TargetPointList[targetPoint.fptr][0] + hover_ned[0];
				dubins_target_buffer[dubins_idx].y = targetPoint.TargetPointList[targetPoint.fptr][1] + hover_ned[1];
				dubins_target_buffer[dubins_idx].yaw_dubins = (yawAutoSetFlag == true ? yaw_command : targetPoint.yawCommandList[targetPoint.fptr]);
				targetPoint.fptr++;
				dubins_idx++;
			}
			dubins_target_init = true; //初始化完成标志位
			dubins_step = 1;
		}
		else{
			if(dubins_segment >= MAX_QUEUE_SIZE - 1){
				return;
			}
			//每一段的初始化
			dubins_curve_buffer[dubins_segment].kind = FALSE;
			dubins_curve_buffer[dubins_segment].p = 0.0f;
			dubins_curve_buffer[dubins_segment].q = 0.0f;
			dubins_curve_buffer[dubins_segment].t = 0.0f;
			dubins_curve_buffer[dubins_segment].dist = 3.4028235E38;
			//dubins曲线规划主要函数
			Dubins_Cal(dubins_target_buffer[dubins_segment],dubins_target_buffer[dubins_segment+1],circle_radius,&dubins_curve_buffer[dubins_segment]);
			dubins_segment++;
			if(dubins_target_buffer[dubins_segment + 1].x == 0.0f && dubins_target_buffer[dubins_segment + 1].y == 0.0f && dubins_target_buffer[dubins_segment + 1].yaw_dubins == 0.0f){
				dubins_mission_moving = true; //dubins曲线执行标志位
				dubins_target_init = false;
			}
		}
		xQueueOverwrite(queueTargetPoint, &targetPoint);
	}
}

void TRAJECTORY::Trajectory_Dubins_Circle_Step(float _circle_velocity, float _radius,u8 left_right, float _angle)
{
	static float init_theta,init_position[3];
	xQueuePeek(queueESKF, &eskf, 0);
    //初始化圆弧轨线参数
	if (first_entry_circle == 1)
	{
		circle_angle = 0.0;
		init_theta = yaw_command;//航向角给定
		init_position[0] = cur_target_pos_ned[0];//初始画圆的点
		init_position[1] = cur_target_pos_ned[1];//初始画圆的点
		init_position[2] = cur_target_pos_ned[2];//初始画圆的点
	}

	if(cur_target_vel_body[0] < _circle_velocity){
		cur_target_vel_body[0] += ACCEL * TRAJECTORY_CONTROL_DT;
	}else{
		cur_target_vel_body[0] = _circle_velocity;
	}
	com_pqr_ned[2] = cur_target_vel_body[0]/_radius;//北东地坐标系角速度指令
	circle_angle += com_pqr_ned[2]*TRAJECTORY_CONTROL_DT;//飞机已经画圆的角度大小

	if (circle_angle >= _angle)//如果已经完成画圆目标，则进入下一阶段
	{
		first_entry_circle = 1;//第一次进入画圆标志位置位，为了每次画圆任务初始化相关参数
		dubins_step++;
		return;
	}
	else
	{
		com_pqr[2] = com_pqr_ned[2] * cos(eskf.Attitude[0]) * cos(eskf.Attitude[1]);//计算机体坐标系下角速度
	}

	if (left_right == 1)//右转画圆
	{
//		yaw_command += com_pqr[2]*TRAJECTORY_CONTROL_DT;
		yaw_command += com_pqr_ned[2]*TRAJECTORY_CONTROL_DT;
		if (yaw_command > PI)
		{
		  yaw_command -= 2*PI;
		}
		cur_target_pos_ned[0] = init_position[0]+_radius*(-sin(init_theta)+sin(yaw_command));
		cur_target_pos_ned[1] = init_position[1]+_radius*(cos(init_theta)-cos(yaw_command));
		cur_target_vel_ned[0] = cur_target_vel_body[0] * cos(yaw_command);
		cur_target_vel_ned[1] = cur_target_vel_body[0] * sin(yaw_command);
	}
	else if (left_right == 2)//左转画圆
	{
//		yaw_command -= com_pqr[2]*TRAJECTORY_CONTROL_DT;
		yaw_command -= com_pqr_ned[2]*TRAJECTORY_CONTROL_DT;
		if (yaw_command < -PI)
		{
			yaw_command += 2*PI;
		}
		cur_target_pos_ned[0] = init_position[0]+_radius*(sin(init_theta)-sin(yaw_command));
		cur_target_pos_ned[1] = init_position[1]+_radius*(-cos(init_theta)+cos(yaw_command));
		cur_target_vel_ned[0] = cur_target_vel_body[0] * cos(yaw_command);
		cur_target_vel_ned[1] = cur_target_vel_body[0] * sin(yaw_command);
	}
	first_entry_circle ++;
	if(first_entry_circle >= 10000)
		first_entry_circle = 200;
}

void TRAJECTORY::Trajectory_Dubins_Straight_Step()
{
	if (cur_target_vel_body[0] < circle_velocity)
	{
		cur_target_vel_body[0] +=  TRAJECTORY_CONTROL_DT * ACCEL ;
	}
	else
	{
		cur_target_vel_body[0] =  circle_velocity;
	}

	how_long_body[0] += cur_target_vel_body[0] * TRAJECTORY_CONTROL_DT;
	cur_target_vel_ned[0] = cur_target_vel_body[0] * cos(yaw_command);
	cur_target_vel_ned[1] = cur_target_vel_body[0] * sin(yaw_command);

	cur_target_pos_ned[0]+= cur_target_vel_ned[0] * TRAJECTORY_CONTROL_DT;
	cur_target_pos_ned[1]+= cur_target_vel_ned[1] * TRAJECTORY_CONTROL_DT;
	if(how_long_body[0]>=dubins_curve_buffer[dubins_segment_moving].p * circle_radius){ //在归一化的基础上再乘上circle_radius系数，才是真正需要走的直线长度
		dubins_step++;
		how_long_body[0] = 0.0f;
	}
}

//解释一下为什么L对应的是向右画圆，R对应的是向左画圆
//这里的公式都是东北天，东北天对应的左转是北东地的右转，因此如果想完成东北天的LSL，那么对应的北东地应该是RSR，因此方向是相反的，这也是一个优化内容
void TRAJECTORY::Trajectory_Dubins_LSL_Step(){
	if(dubins_step == 1){ //完成dubins曲线初始化后，dubins_step = 1；
		heli_step = circleStep;
		Trajectory_Dubins_Circle_Step(circle_velocity, circle_radius, 1, dubins_curve_buffer[dubins_segment_moving].t);
	}else if(dubins_step == 2){
		heli_step = speedUpStep;
		Trajectory_Dubins_Straight_Step();
	}else if(dubins_step == 3){
		heli_step = circleStep;
		Trajectory_Dubins_Circle_Step(circle_velocity, circle_radius, 1, dubins_curve_buffer[dubins_segment_moving].q);
	}
	if(dubins_step ==4){
		dubins_segment_moving++;
		dubins_step = 1;
	}
}

void TRAJECTORY::Trajectory_Dubins_LSR_Step(){
	if(dubins_step == 1){
		heli_step = circleStep;
		Trajectory_Dubins_Circle_Step(circle_velocity, circle_radius, 1, dubins_curve_buffer[dubins_segment_moving].t);
	}else if(dubins_step == 2){
		heli_step = speedUpStep;
		Trajectory_Dubins_Straight_Step();
	}else if(dubins_step == 3){
		heli_step = circleStep;
		Trajectory_Dubins_Circle_Step(circle_velocity, circle_radius, 2, dubins_curve_buffer[dubins_segment_moving].q);
	}
	if(dubins_step ==4){
		dubins_segment_moving++;
		dubins_step = 1;
	}
}

void TRAJECTORY::Trajectory_Dubins_RSL_Step(){
	if(dubins_step == 1){
		heli_step = circleStep;
		Trajectory_Dubins_Circle_Step(circle_velocity, circle_radius, 2, dubins_curve_buffer[dubins_segment_moving].t);
	}	else if(dubins_step == 2){
		heli_step = speedUpStep;
		Trajectory_Dubins_Straight_Step();
	}else if(dubins_step == 3){
		heli_step = circleStep;
		Trajectory_Dubins_Circle_Step(circle_velocity, circle_radius, 1, dubins_curve_buffer[dubins_segment_moving].q);
	}
	if(dubins_step ==4){
		dubins_segment_moving++;
		dubins_step = 1;
	}
}

void TRAJECTORY::Trajectory_Dubins_RSR_Step(){
	if(dubins_step == 1){
		heli_step = circleStep;
		Trajectory_Dubins_Circle_Step(circle_velocity, circle_radius, 2, dubins_curve_buffer[dubins_segment_moving].t);
	}	else if(dubins_step == 2){
		heli_step = speedUpStep;
		Trajectory_Dubins_Straight_Step();
	}else if(dubins_step == 3){
		heli_step = circleStep;
		Trajectory_Dubins_Circle_Step(circle_velocity, circle_radius, 2, dubins_curve_buffer[dubins_segment_moving].q);
	}
	if(dubins_step ==4){
		dubins_segment_moving++;
		dubins_step = 1;
	}
}

void TRAJECTORY::Trajectory_Dubins_RLR_Step(){
	if(dubins_step == 1){
		heli_step = circleStep;
		Trajectory_Dubins_Circle_Step(circle_velocity, circle_radius, 2, dubins_curve_buffer[dubins_segment_moving].t);
	}	else if(dubins_step == 2){
		heli_step = circleStep;
		Trajectory_Dubins_Circle_Step(circle_velocity, circle_radius, 1, dubins_curve_buffer[dubins_segment_moving].p);
	}else if(dubins_step == 3){
		heli_step = circleStep;
		Trajectory_Dubins_Circle_Step(circle_velocity, circle_radius, 2, dubins_curve_buffer[dubins_segment_moving].q);
	}
	if(dubins_step ==4){
		dubins_segment_moving++;
		dubins_step = 1;
	}
}

void TRAJECTORY::Trajectory_Dubins_LRL_Step(){
	if(dubins_step == 1){
		heli_step = circleStep;
		Trajectory_Dubins_Circle_Step(circle_velocity, circle_radius, 1, dubins_curve_buffer[dubins_segment_moving].t);
	}	else if(dubins_step == 2){
		heli_step = circleStep;
		Trajectory_Dubins_Circle_Step(circle_velocity, circle_radius, 2, dubins_curve_buffer[dubins_segment_moving].p);
	}else if(dubins_step == 3){
		heli_step = circleStep;
		Trajectory_Dubins_Circle_Step(circle_velocity, circle_radius, 1, dubins_curve_buffer[dubins_segment_moving].q);
	}
	if(dubins_step == 4){
		dubins_segment_moving++;
		dubins_step = 1;
	}
}



void TRAJECTORY::Dubins_Cal(Dubins_Target start_point,Dubins_Target end_point,float radius,Dubins_Curve *result)
{
	float dx = end_point.x - start_point.x;
	float dy = end_point.y - start_point.y;
	float Dist = sqrt(dx*dx+dy*dy);
	/*
		6种dubins曲线情况在计算t p q时，是在以下假设情况下进行的：
		1. Pi(0,0,alpha)
		2. Pf(d,0,beta)
		3. Rmin = 1
		因此对于一般的Pi(x1,x2,alpha1), Pf(x2,y2,beta1),  Rmin = R，需要先进行坐标的平移、旋转、缩放，变成123这种形式，才能套公式
	*/
	float d = Dist/radius; //归一化d
	float theta = atan2(dy,dx); //旋转坐标系的角度theta
	PIto2PI(&theta);
	float alpha = start_point.yaw_dubins - theta; //在辅助坐标系（旋转后的坐标系）下的yaw
	PIto2PI(&alpha);
	float beta  = end_point.yaw_dubins - theta; //在辅助坐标系（旋转后的坐标系）下的yaw
	PIto2PI(&beta);

	dubins_LSL(alpha,beta,d,result);
	dubins_LSR(alpha,beta,d,result);
	dubins_RSL(alpha,beta,d,result);
	dubins_RSR(alpha,beta,d,result);
	dubins_RLR(alpha,beta,d,result);
	dubins_LRL(alpha,beta,d,result);
}

void TRAJECTORY::dubins_LSL(float alpha,float beta,float d,Dubins_Curve *result)
{
	float tmp0 = d + sin(alpha) - sin(beta);
	float p_squared = 2 + (d*d) -(2*cos(alpha - beta)) + (2*d*(sin(alpha) - sin(beta)));
	if( p_squared < 0.0f )
		return;
	else{
		float tmp1 = atan2((cos(beta)-cos(alpha)), tmp0 );
		float t = -alpha + tmp1;
		PIto2PI(&t);
		float p = sqrt( p_squared );
		float q = beta - tmp1;
		PIto2PI(&q);
		float dist_temp = t+p+q; //这个是没有乘R之前的dist_temp，实际走的路程需要dist_temp * R
		if(result->dist>dist_temp){
			result->t = t;
			result->p = p;
			result->q = q;
			result->dist = dist_temp;
			result->kind = LSL;
			return;
		}
	}
}

void TRAJECTORY::dubins_LSR(float alpha,float beta,float d,Dubins_Curve *result)
{
    float p_squared = -2 + (d*d) + (2*cos(alpha - beta)) + (2*d*(sin(alpha)+sin(beta)));
    if( p_squared < 0.0f )
      return;
    else{
      float p = sqrt( p_squared );
      float tmp2 = atan2( (-cos(alpha)-cos(beta)), (d+sin(alpha)+sin(beta)) ) - atan2(-2.0, p);
      float t = -alpha + tmp2;
			PIto2PI(&t);
      float q = - beta + tmp2;
			PIto2PI(&q);
			float dist_temp = t+p+q;
			if(result->dist>dist_temp){
				result->t = t;
				result->p = p;
				result->q = q;
				result->dist = dist_temp;
				result->kind = LSR;
				return;
			}
    }
}

void TRAJECTORY::dubins_RSL(float alpha,float beta,float d,Dubins_Curve *result)
{
    float p_squared = (d*d) -2 + (2*cos(alpha - beta)) - (2*d*(sin(alpha)+sin(beta)));
    if( p_squared < 0.0f )
		return;
    else{
		float p = sqrt( p_squared );
		float tmp2 = atan2( (cos(alpha)+cos(beta)), (d-sin(alpha)-sin(beta)) ) - atan2(2.0, p);
		float t = alpha - tmp2;
		PIto2PI(&t);
		float q = beta - tmp2;
		PIto2PI(&q);
		float dist_temp = t+p+q;
		if(result->dist>dist_temp){
			result->t = t;
			result->p = p;
			result->q = q;
			result->dist = dist_temp;
			result->kind = RSL;
			return;
		}
    }
}

void TRAJECTORY::dubins_RSR(float alpha,float beta,float d,Dubins_Curve *result)
{
	float tmp0 = d-sin(alpha)+sin(beta);
    float p_squared = 2 + (d*d) -(2*cos(alpha - beta)) + (2*d*(sin(beta)-sin(alpha)));
    if( p_squared < 0.0f )
		return;
    else{
		float tmp1 = atan2( (cos(alpha)-cos(beta)), tmp0 );
		float t = alpha - tmp1;
		PIto2PI(&t);
		float p = sqrt( p_squared );
		float q = -beta + tmp1;
		PIto2PI(&q);
		float dist_temp = t+p+q;
		if(result->dist>dist_temp){
			result->t = t;
			result->p = p;
			result->q = q;
			result->dist = dist_temp;
			result->kind = RSR;
			return;
		}
    }
}

void TRAJECTORY::dubins_RLR(float alpha,float beta,float d,Dubins_Curve *result)
{
	float tmp_rlr = (6.0f - d*d + 2*cos(alpha - beta) + 2*d*(sin(alpha)-sin(beta))) / 8.0f;
    if(fabs(tmp_rlr) > 1.0f)
      return;
    else{
		float p = acos(tmp_rlr); //float p = 2*PI - acos( tmp_rlr );
		PIto2PI(&p);
		float t = alpha - atan2( cos(alpha)-cos(beta), d-sin(alpha)+sin(beta)) + p/2.0f;
		PIto2PI(&t);
		float q = (alpha - beta - t + p);
		PIto2PI(&q);
		float dist_temp = t+p+q;
		if(result->dist>dist_temp){
			result->t = t;
			result->p = p;
			result->q = q;
			result->dist = dist_temp;
			result->kind = RLR;
			return;
		}
	}
}

void TRAJECTORY::dubins_LRL(float alpha,float beta,float d,Dubins_Curve *result)
{
	float tmp_lrl = (6.0f - d*d + 2*cos(alpha - beta) + 2*d*(- sin(alpha) + sin(beta))) / 8.0f;
    if(fabs(tmp_lrl) > 1.0f)
      return;
    else{
		float p = acos( tmp_lrl ); //p = acos( tmp_lrl );
		PIto2PI(&p);
		float t = -alpha + atan2( -cos(alpha)+cos(beta), d+sin(alpha)-sin(beta)) + p/2.0f; //-alpha + atan2( cos(alpha)-cos(beta), d+sin(alpha)-sin(beta)) + p/2.0f;
		PIto2PI(&t);
		float q = beta - alpha - t + p;
		PIto2PI(&q);
		float dist_temp = t+p+q;
		if(result->dist>dist_temp){
			result->t = t;
			result->p = p;
			result->q = q;
			result->dist = dist_temp;
			result->kind = LRL;
			return;
		}
    }
}

void TRAJECTORY::Trajectory_Target_Receive(void)
{
	xQueuePeek(queueTargetPoint, &targetPoint, 0);
	if(targetPoint.isUpdate == true)      //当从地面站输入坐标点时才会变成true
	{
		if(targetPoint.TargetPointList[targetPoint.fptr][3] != -1.0f){
			trajectory.first_receive_target++;				//增加目标点
		}
		targetPoint.isUpdate = false;
	}
	xQueueOverwrite(queueTargetPoint, &targetPoint);
}

void TRAJECTORY::minimumSnap_Planning(){
	if(trajectory_mod == MinimumSnap){
		if(minimumSnapPlanningFinish == false){
			/*
			 * 注意：
			 * 	1. 将轨迹点控制在<=8个以内
			 * 	8个点，7条轨迹：0.28s
			 * 	7个点，6条轨迹：0.18s
			 * 	6个点，5条轨迹：0.126s
			 * 	5个点，4条轨迹：0.06s
			 * */
//			MatXd waypoint_posi(8, 2);
//			waypoint_posi <<
//				1, 2,
//				3, 4,
//				5, 6,
//				7, 8,
//				9, 10,
//				11, 12,
//				13, 14,
//				15, 16;
			MatXd waypoint_posi(4, 2);
			waypoint_posi <<
				7,                5,
				20.9509724121217, 13.5413991920978,
				33.1686547830449, 34.5563307360413,
				42,               40;
			startPoint[0] = waypoint_posi(0, 0);
			startPoint[1] = waypoint_posi(0, 1);
			/*
			 * 比如说，minimum snap现有4个点，分别是：
			 * 	7,                5,
				20.9509724121217, 13.5413991920978,
				33.1686547830449, 34.5563307360413,
				42,               40;
			 * 则通过PosVelYaw[i] - startPoint[i]将起点(7, 5)变成了(0, 0)，这是相对坐标
			 * 再加上hover_ned[i]，就变成了NED坐标了
			 * */
			for(int i = 0; i < waypoint_posi.rows(); ++i){
				waypoint_posi(i, 0) -= startPoint[0];
				waypoint_posi(i, 1) -= startPoint[1];
			}
			sTIM TIM;
			Tim_Calc(&TIM);
			VecXd waypoint_vel = VecXd::Zero(waypoint_posi.rows());
			VecXd waypoint_accel = VecXd::Zero(waypoint_posi.rows());
			allocateTime = minimumSnapObj.AllocateTime(waypoint_posi);
			polynomial_coeff_x = minimumSnapObj.SolveQPClosedForm(waypoint_posi.col(0), waypoint_vel, waypoint_accel, allocateTime); //8个系数
			polynomial_coeff_y = minimumSnapObj.SolveQPClosedForm(waypoint_posi.col(1), waypoint_vel, waypoint_accel, allocateTime);
			polynomial_coeff_vx = minimumSnapObj.polydiff(polynomial_coeff_x); //7个系数
			polynomial_coeff_vy = minimumSnapObj.polydiff(polynomial_coeff_y);
			polynomial_coeff_ax = minimumSnapObj.polydiff(polynomial_coeff_vx); //6个系数
			polynomial_coeff_ay = minimumSnapObj.polydiff(polynomial_coeff_vy);
			//debug用
//			int row = polynomial_coeff_x.rows();
//			int col = polynomial_coeff_x.cols();
//			double test[row][col];
//			for(int i = 0; i < row; ++i){
//				for(int j = 0; j < col; ++j){
//					test[i][j] = polynomial_coeff_x(i, j);
//				}
//			}
//			for(int i = 0; i < row; ++i){
//				for(int j = 0; j < col; ++j){
//					test[i][j] = polynomial_coeff_y(i, j);
//				}
//			}
//			row = polynomial_coeff_vx.rows();
//			col = polynomial_coeff_vy.cols();
//			double test2[row][col];
//			for(int i = 0; i < row; ++i){
//				for(int j = 0; j < col; ++j){
//					test2[i][j] = polynomial_coeff_vx(i, j);
//				}
//			}
//			for(int i = 0; i < row; ++i){
//				for(int j = 0; j < col; ++j){
//					test2[i][j] = polynomial_coeff_vy(i, j);
//				}
//			}
//			row = polynomial_coeff_ax.rows();
//			col = polynomial_coeff_ay.cols();
//			double test3[row][col];
//			for(int i = 0; i < row; ++i){
//				for(int j = 0; j < col; ++j){
//					test3[i][j] = polynomial_coeff_ax(i, j);
//				}
//			}
//			for(int i = 0; i < row; ++i){
//				for(int j = 0; j < col; ++j){
//					test3[i][j] = polynomial_coeff_ay(i, j);
//				}
//			}
			Tim_Calc(&TIM);
			minimumSnapExeTime = (float)TIM.OUT / 1000000; //单位：秒
			minimumSnapPlanningFinish = true;
			segmentNum = waypoint_posi.rows() - 1;
		}
	}
}

void TRAJECTORY::Trajectory_MinimumSnap_Mode_Step(){
	if(minimumSnapPlanningFinish == true){
		if(segmentNum < 1) return; //至少有一段轨迹
		if(minimumSnapStepFlag == msInit){ //step1:初始化，计算起点的航向角
			double PosVelAccYaw[7] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
			//这里选取的时间为TRAJECTORY_CONTROL_DT的原因是如果选取了时间0，那么计算出来的航向角为0（atan(vy / vx)中的vy与vx都是0，失效），因此计算起点的航向角时需要以0.01s计算而不是0！
			minimumSnapObj.getCommandPosVelAccYaw(PosVelAccYaw, polynomial_coeff_x, polynomial_coeff_y, polynomial_coeff_vx, polynomial_coeff_vy, polynomial_coeff_ax, polynomial_coeff_ay, curSegment, TRAJECTORY_CONTROL_DT);
			end_heading = PosVelAccYaw[6];
			if (end_heading - yaw_command >= PI){
				isturn = 1;
			}
			else if (end_heading - yaw_command <= -PI){
				isturn = 2;
			}
			else if (end_heading - yaw_command >= 0.00f && end_heading - yaw_command < PI){
				isturn = 3;     //
			}
			else if (end_heading - yaw_command < 0.00f && end_heading - yaw_command > -PI){
				isturn = 4;    //
			}
			minimumSnapStepFlag = msTurnHeadStep;
		}
		else if(minimumSnapStepFlag == msTurnHeadStep){ //step1 : 掉头，将机头对准第一段轨迹连线部分
			Trajectory_Turning_Head_Step();
			if(turn_accel_flag==true) //调转机头成功标志位
			{
				turn_accel++;
			}
			if(turn_accel==300) //在调转机头成功后，再等待10ms * 300 = 3s
			{
				turn_accel=0;
				turn_accel_flag=false;
				minimumSnapStepFlag = msFeedingPointStep;
			}
		}
		else if(minimumSnapStepFlag == msFeedingPointStep){ //step2：主体部分，开始喂点，包括p、v、yaw
			if(curTime >= allocateTime(curSegment)){
				curTime = 0.0;
				curSegment++;
				if(curSegment >= segmentNum){
					minimumSnapStepFlag = msBrakeDownStep;
					return;
				}
			}
			double PosVelAccYaw[7] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
			minimumSnapObj.getCommandPosVelAccYaw(PosVelAccYaw, polynomial_coeff_x, polynomial_coeff_y, polynomial_coeff_vx, polynomial_coeff_vy, polynomial_coeff_ax, polynomial_coeff_ay, curSegment, curTime);
			for(int i = 0; i < 2; ++i){ //当前只对二维px、py、vx、vy、ax、ay做规划，pz、vz、az默认都是0
				cur_target_pos_ned[i] = PosVelAccYaw[i] + hover_ned[i];
				cur_target_vel_ned[i] = PosVelAccYaw[i + 2];
				cur_target_acc_ned[i] = PosVelAccYaw[i + 4];
			}
			cur_target_pos_ned[2] = 0.0f + hover_ned[2];
			cur_target_vel_ned[2] = 0.0f;
			if(!(curSegment == 0 && curTime == 0)){ //当且仅当在第0段轨迹时间0时不需要计算航向角，而是按照初始化时的航向角
				yaw_command = PosVelAccYaw[6];
			}
			curTime += TRAJECTORY_CONTROL_DT;
		}
		else if(minimumSnapStepFlag == msBrakeDownStep){ //step3：算法结束，进行刹车操作
			cur_target_vel_body[0] = sqrt(cur_target_vel_ned[0] * cur_target_vel_ned[0] + cur_target_vel_ned[1] * cur_target_vel_ned[1]);
			Trajectory_Brake_Down_Step();
		}
		else{
			Trajectory_Hover_Step();
		}
	}
}

void TRAJECTORY::Trajectory_Reset()
{
	xQueuePeek(queueESKF, &eskf, 0);
	//trajectory部分
	trajectory_mod = Dubins;//Circle  Point_to_point  Hover Dubins MinimumSnap
	heli_step = hoverStep;
	RTL_Orignal_flag = 0; //RTL
	RTL_Orignal_update_flag = 1;//RTL
	RTL_First_switch = 1;//RTL
	end_heading = eskf.Attitude[2]; //将当前的航向角作为掉头后的方向
	yaw_command = eskf.Attitude[2]; //2021.12.30
	hover_time = FLY_TIME;
	isturn = 0;
	first_receive_target = 0;
	turn_accel = 0; //在调头后开始计时，3s后再直线飞行
	turn_accel_flag = false; //掉头完成标志位
	first_entry_brake = 1; //第一次进入刹车函数的初始化标志位
	first_entry_circle = 1; //第一次进入圆弧函数的初始化标志位
	circle_velocity = 3.0f;//圆弧轨线速度
	circle_radius_test = 8.0f; //圆弧半径
	circle_finish_flag = false; //圆弧完成标志位
	circleNum = 5; //画圆圈数
	brake_distance = 0.0f;
	for(int i = 0; i < 3 ; ++i)
	{
		hover_ned[i] = eskf.Pos[i]; //在切到全自控的瞬间，将当前GPS测出的NED坐标作为基准悬停点
		next_target_ned[i] = hover_ned[i];
		cur_target_pos_ned[i] = hover_ned[i];
		cur_target_vel_ned[i] = 0.0f;
		com_pqr[i]      = 0.0f;
		com_theta[i]    = 0.0f;
		top_speed_body[i] = 0.0f;
		how_long_body[i] = 0.0f;
	}
	//dubins
	dubins_segment = 0; //dubins曲线规划段数
	dubins_segment_moving = 0; //正在执行的dubins曲线段数
	dubins_target_init = false;
	circle_radius = 10;	//dubins圆弧半径
	dubins_mission_moving = false;
	clearFlag = false;
	dubins_idx = 1;
	yawAutoSetFlag = false;
	for(int i = 0; i < MAX_QUEUE_SIZE; ++i){
		dubins_target_buffer[i].x = 0.0f;
		dubins_target_buffer[i].y = 0.0f;
		dubins_target_buffer[i].yaw_dubins = 0.0f;
	}
	//minimumSnap
	minimumSnapPlanningFinish = false;
	minimumSnapStepFlag = msInit;
	curSegment = 0;
	curTime = 0.0;
}

extern "C" void dubinsPlanning_main(void *argument)
{
	osDelay(1000);
	for(;;)
	{
		osSemaphoreAcquire(semDubinsPlanning,0xffffffff);
		trajectory.Dubins_Planning();
	}
}

extern "C" void minimumSnapPlanning_main(void *argument)
{
	osDelay(2000);
	for(;;)
	{
		osSemaphoreAcquire(semMinimumSnapPlanning, 0xffffffff);
		trajectory.minimumSnap_Planning();
	}
}
